#include <iostream>
#include <cassert>

#include <CGAL/Exact_predicates_exact_constructions_kernel.h>
#include <CGAL/Constrained_Delaunay_triangulation_2.h>
#include <CGAL/Triangulation_face_base_with_info_2.h>
#include <CGAL/Constrained_triangulation_plus_2.h>
#include <CGAL/Polygon_2.h>
#include <CGAL/Triangle_2.h>
#include <CGAL/Line_2.h>

#include <simgear/debug/logstream.hxx>

#include "tg_polygon.hxx"
#include "tg_misc.hxx"
#include "tg_shapefile.hxx"

/* determining if a face is within the reulting poly */
struct FaceInfo2
{
  FaceInfo2() {}
  int nesting_level;

  bool in_domain(){
    return nesting_level%2 == 1;
  }
};

typedef CGAL::Exact_predicates_exact_constructions_kernel         K;
typedef CGAL::Triangulation_vertex_base_2<K>                      Vb;
typedef CGAL::Triangulation_face_base_with_info_2<FaceInfo2,K>    Fbb;
typedef CGAL::Constrained_triangulation_face_base_2<K,Fbb>        Fb;
typedef CGAL::Triangulation_data_structure_2<Vb,Fb>               TDS;
typedef CGAL::Exact_intersections_tag                             Itag;
typedef CGAL::Constrained_Delaunay_triangulation_2<K, TDS, Itag>  CDT;
typedef CGAL::Constrained_triangulation_plus_2<CDT>               CDTPlus;
typedef CDTPlus::Point                                            Point;
typedef CGAL::Polygon_2<K>                                        Polygon_2;
typedef CGAL::Line_2<K>                                           Line;
typedef CGAL::Triangle_2<K>                                       Triangle_2;


typedef boost::tuple<Point,CDTPlus::Vertex_handle>                PointHandle;
typedef CGAL::Search_traits_2<K>                                  SearchTraitsBase;
typedef CGAL::Search_traits_adapter<PointHandle,CGAL::Nth_of_tuple_property_map<0, PointHandle>,SearchTraitsBase> SearchTraits;

typedef CGAL::Fuzzy_sphere<SearchTraits>                          SearchFuzzyCir;
typedef CGAL::Kd_tree<SearchTraits>                               SearchTree;





#if 0
struct constraint 
{
public:
    constraint( CDTPlus::Vertex_handle s, CDTPlus::Vertex_handle t ) : 
        source(s), target(t) {}

    CDTPlus::Vertex_handle  source;
    CDTPlus::Vertex_handle  target;
};
#endif

#if 0
static void AddPoint( const SGGeod& p, SearchTree& tree, CDTPlus& cdt ) {
    std::list<PointHandle>  searchResults;
    
    // first - create a search node
    Point           pt( p.getLongitudeDeg(), p.getLatitudeDeg() );
    SearchFuzzyCir  query_circle( pt, 0.0000001 );  // approx 1 cm
    
    // perform the query
    searchResults.clear();
    tree.search(std::back_inserter( searchResults ), query_circle);
    
    CDTPlus::Vertex_handle h;
    if ( searchResults.empty() ) {
        // no node here - add a new one
        // index = tg_node_list.size();
        // double e = p.getElevationM();
        
        //tg_node_list.push_back( TGNode(p,t) );
        h = cdt.insert( pt );
        PointHandle data(pt, h);
        
        tree.insert(data);            
    } else {
        // we found a node - don't insert again
    }    
}
#endif

#if 0
static CDTPlus::Vertex_handle Lookup(  const SGGeod& p, SearchTree& tree ) {
    std::list<PointHandle>  searchResults;
    
    // first - create a search node
    Point           pt( p.getLongitudeDeg(), p.getLatitudeDeg() );
    SearchFuzzyCir  query_circle( pt, 0.0000001 );  // approx 1 cm
    
    // perform the query
    searchResults.clear();
    tree.search(std::back_inserter( searchResults ), query_circle);
    
    CDTPlus::Vertex_handle h;
    if ( !searchResults.empty() ) {
        // we found a node - use it
        std::list<PointHandle>::const_iterator it = searchResults.begin();
        h = (CDTPlus::Vertex_handle)boost::get<1>(*it);
        //SG_LOG( SG_GENERAL, SG_ALERT, "Found node for " << p << " at " << (Point)boost::get<0>(*it) );        
    } else {
        // we found a node - don't insert again
        SG_LOG( SG_GENERAL, SG_ALERT, "CAN'T FIND NODE" );        
    }    
    
    return h;
}
#endif

static void tg_mark_domains(CDT& ct, CDT::Face_handle start, int index, std::list<CDT::Edge>& border )
{
    if(start->info().nesting_level != -1) {
        return;
    }

    std::list<CDTPlus::Face_handle> queue;
    queue.push_back(start);

    while( !queue.empty() ){
        CDTPlus::Face_handle fh = queue.front();
        queue.pop_front();
        if(fh->info().nesting_level == -1) {
            fh->info().nesting_level = index;
            for(int i = 0; i < 3; i++) {
                CDTPlus::Edge e(fh,i);
                CDTPlus::Face_handle n = fh->neighbor(i);
                if(n->info().nesting_level == -1) {
                    if(ct.is_constrained(e)) border.push_back(e);
                    else queue.push_back(n);
                }
            }
        }
    }
}

//explore set of facets connected with non constrained edges,
//and attribute to each such set a nesting level.
//We start from facets incident to the infinite vertex, with a nesting
//level of 0. Then we recursively consider the non-explored facets incident
//to constrained edges bounding the former set and increase the nesting level by 1.
//Facets in the domain are those with an odd nesting level.
static void tg_mark_domains(CDTPlus& cdt)
{
    for(CDTPlus::All_faces_iterator it = cdt.all_faces_begin(); it != cdt.all_faces_end(); ++it){
        it->info().nesting_level = -1;
    }

    int index = 0;
    std::list<CDTPlus::Edge> border;
    tg_mark_domains(cdt, cdt.infinite_face(), index++, border);
    while(! border.empty()) {
        CDTPlus::Edge e = border.front();
        border.pop_front();
        CDTPlus::Face_handle n = e.first->neighbor(e.second);
        if(n->info().nesting_level == -1) {
            tg_mark_domains(cdt, n, e.first->info().nesting_level+1, border);
        }
    }
}

static void tg_insert_polygon(CDTPlus& cdt,const Polygon_2& polygon)
{
    if ( polygon.is_empty() ) return;

    CDTPlus::Vertex_handle v_prev=cdt.insert(*CGAL::cpp0x::prev(polygon.vertices_end()));
    for (Polygon_2::Vertex_iterator vit=polygon.vertices_begin(); vit!=polygon.vertices_end();++vit) {
        CDTPlus::Vertex_handle vh=cdt.insert(*vit);
        cdt.insert_constraint(vh,v_prev);
        v_prev=vh;
    }
}


#if 0
static void save_cgal_debug( unsigned long id, SearchTree& tree, const std::vector<constraint>&  constraints )
{
    char filename[64];
    sprintf( filename, "tridata_%06lu", id );
    
    std::vector<CDTPlus::Vertex_handle> handles;
    
    // save datafiles for CGAL post
    std::ofstream output_file(filename);
    if (!output_file.is_open()) {
        std::cerr << "Failed to open the " << "./output_polys.txt" << std::endl;
        exit(0);
    }
    
    SearchTree::iterator it1 = tree.begin();
    output_file << tree.size() << std::endl;
    while ( it1 != tree.end() ) {
        output_file << std::setprecision(16) << (Point)(boost::get<0>(*it1)) << std::endl;
        handles.push_back( (CDTPlus::Vertex_handle)(boost::get<1>(*it1)) );
        
        it1++;
    }
    
    // then write the constraints as indicies to points
    output_file << constraints.size() << std::endl;
    for ( unsigned int i=0; i<constraints.size(); i++ ) {
        // find the index of the handles
        int s = -1, t = -1;
        
        for ( unsigned int j=0; j<handles.size(); j++ ) {
            if ( constraints[i].source == handles[j] ) {
                s = j;
            }
            if ( constraints[i].target == handles[j] ) {
                t = j;
            }
            
            if ( (s >= 0) && (t >= 0) ) {
                break;
            }
        }
        
        output_file << s << " " << t << std::endl;
    }
    
    // output_file << std::setprecision(16) << P;            
    output_file.close();
}
#endif

#if 0
static void insert_constraints(CDTPlus& cdt, const std::vector<constraint>&  constraints, char* datasource, unsigned long id)
{
    char layer[256];
    char cons[128];
    
    sprintf( layer, "constraints_%06lu", id );
    
    Line curLine, nxtLine;
    SGGeod gSource, gTarget;
    CDTPlus::Vertex_handle hSource, hTarget;
    
    int constraint_num = 1;
    for ( unsigned int i=0; i<constraints.size(); i++ ) {        
        // done add equal constraint lines - combine the segments
        curLine = Line(constraints[i].source->point(), constraints[i].target->point() );
        
        if ( i > 0 ) {
            if ( curLine != nxtLine ) {
                sprintf( cons, "cons_%04d", constraint_num++ );
                
                gSource = SGGeod::fromDeg( CGAL::to_double( hSource->point().x() ),
                                           CGAL::to_double( hSource->point().y() ) );
                gTarget = SGGeod::fromDeg( CGAL::to_double( hTarget->point().x() ),
                                           CGAL::to_double( hTarget->point().y() ) );
        
                tgShapefile::FromSegment( tgSegment(gSource, gTarget), false, datasource, layer, cons );

                // new constraint - add the previous
                cdt.insert_constraint( hSource, hTarget );
                
                // now remember this new one, but don't add it yet
                hSource = constraints[i].source;
                hTarget = constraints[i].target;                
            } else {
                // cur constraint is really part of last constraint - extend it
                hTarget = constraints[i].target;
                SG_LOG( SG_GENERAL, SG_INFO, "Tess with extra " << id << " EXTENDING CONSTRAINT" );
            }
        } else {
            // first constraint - don't add it, just remember it
            hSource = constraints[i].source;
            hTarget = constraints[i].target;
        }   
        
        nxtLine = curLine;
    }
    
    // add the final constraint
    sprintf( cons, "cons_%04d", constraint_num++ );
    
    gSource = SGGeod::fromDeg( CGAL::to_double( hSource->point().x() ),
                            CGAL::to_double( hSource->point().y() ) );
    gTarget = SGGeod::fromDeg( CGAL::to_double( hTarget->point().x() ),
                            CGAL::to_double( hTarget->point().y() ) );

    tgShapefile::FromSegment( tgSegment(gSource, gTarget), false, datasource, layer, cons );

    cdt.insert_constraint( hSource, hTarget );    
}
#endif

#if 0                
void tgPolygon::Tesselate( const std::vector<SGGeod>& extra, bool debug )
{
    SearchTree  tree;
    CDTPlus     cdt;

    char datasource[64];
    char layer[256];
    std::vector<SGGeod> polynodes;
    
    // gather all nodes in the poly
    if ( contours.size() != 0 ) {
        for ( unsigned int c = 0; c < contours.size(); c++ ) {
            tgContour contour = contours[c];
            for (unsigned int n = 0; n < contour.GetSize(); n++ ) {
                SGGeod node = contour.GetNode(n);
                polynodes.push_back( node );
            }
        }
    }        
    
    debug = true;
    if ( debug ) {
        sprintf(datasource, "./tridbg/tri_%06u", id );

        sprintf( layer, "poly_%06u", id );
        tgShapefile::FromPolygon(*this, false, false, datasource, layer, "polygon" );
        sprintf( layer, "extra_%06u", id );
        tgShapefile::FromGeodList( extra, false, datasource, layer, "extra" );
        sprintf( layer, "polynodes_%06u", id );
        tgShapefile::FromGeodList( polynodes, false, datasource, layer, "extra" );
    }
    
    SG_LOG( SG_GENERAL, SG_INFO, "Tess with extra " << id << " extra nodes " << extra.size() << " poly nodes " << polynodes.size() );
    
    //////////////// ADD UNIQUE NODES with handles //////////////////////
    // we need to keep track of added points - we may have dupes in extra
    
    // first - dump the poly we are tesselating, along with all of its vertices_begin
    // Bail right away if polygon is empty
    if ( contours.size() != 0 ) {
        // First, Add all the extra points
        for (unsigned int n = 0; n < extra.size(); n++) {
            AddPoint( extra[n], tree, cdt );
        }
        
        // then insert each polygon point 
        for ( unsigned int c = 0; c < contours.size(); c++ ) {
            tgContour contour = contours[c];

            for (unsigned int n = 0; n < contour.GetSize(); n++ ) {
                SGGeod node = contour.GetNode(n);
                AddPoint( node, tree, cdt );
            }
        }
        
        SG_LOG( SG_GENERAL, SG_INFO, "  Added " << tree.size() << " nodes for " << id );
                
        // Now add the constraints
        std::vector<constraint> constraints;
        for ( unsigned int c = 0; c < contours.size(); c++ ) {
            tgContour contour = contours[c];
            CDTPlus::Vertex_handle source, target;
            unsigned int n;
            
            // compare current with next segment - combine if paralell
            for ( n = 0; n < contour.GetSize()-1; n++ ) {
                source = Lookup( contour.GetNode(n), tree );
                target = Lookup( contour.GetNode(n+1), tree );
                constraints.push_back( constraint( source, target ) );
            }
            source = Lookup( contour.GetNode(n), tree );
            source = Lookup( contour.GetNode(0), tree );
            constraints.push_back( constraint( source, target ) );
        }

        save_cgal_debug( id, tree, constraints );
        
        insert_constraints( cdt, constraints, datasource, id );
        SG_LOG( SG_GENERAL, SG_INFO, "  Added " << constraints.size() << " constraints for " << id );
        
        /* make conforming - still has an issue, and can't be compiled with exact_construction kernel */
        // CGAL::make_conforming_Delaunay_2( cdt );
        tg_mark_domains( cdt );

        int count=0;
        for (CDTPlus::Finite_faces_iterator fit=cdt.finite_faces_begin(); fit!=cdt.finite_faces_end(); ++fit) {
            if ( fit->info().in_domain() ) {
                Triangle_2 tri = cdt.triangle(fit);

                SGGeod p0 = SGGeod::fromDeg( to_double(tri.vertex(0).x()), to_double(tri.vertex(0).y()) );
                SGGeod p1 = SGGeod::fromDeg( to_double(tri.vertex(1).x()), to_double(tri.vertex(1).y()) );
                SGGeod p2 = SGGeod::fromDeg( to_double(tri.vertex(2).x()), to_double(tri.vertex(2).y()) );

                /* Check for Zero Area before inserting */
                if ( !SGGeod_isEqual2D( p0, p1 ) && !SGGeod_isEqual2D( p1, p2 ) && !SGGeod_isEqual2D( p0, p2 ) ) {
                    AddTriangle( p0, p1, p2 );
                    ++count;
                } else {
                    SG_LOG( SG_GENERAL, SG_BULK, "tesselation dropping ZAT" );
                    ++count;
                }
                
            }
        }

        SG_LOG( SG_GENERAL, SG_INFO, "  Got " << count << " triangles for " << id );        
    }
}
#endif

void tgPolygon::Tesselate(bool debug)
{
    CDTPlus cdt;
    std::vector<SGGeod> geods;
    char layer[256];
    
    SG_LOG( SG_GENERAL, SG_DEBUG, "Tess " << id );
    
    // first - dump the poly we are tesselating, along with all of its vertices_begin
    if ( debug ) {
        sprintf( layer, "poly_%03d", id );
        //tgShapefile::FromPolygon(*this, false, false, "./tridbg", layer, "polygon" );
    }
    
    // Bail right away if polygon is empty
    if ( contours.size() != 0 ) {
        // insert each polygon as a constraint into the triangulation
        for ( unsigned int c = 0; c < contours.size(); c++ ) {
            tgContour contour = contours[c];
            Polygon_2 poly;

            for (unsigned int n = 0; n < contour.GetSize(); n++ ) {
                SGGeod node = contour.GetNode(n);
                SG_LOG( SG_GENERAL, SG_DEBUG, "Tess : Adding GEOD " << node);
                poly.push_back( Point( node.getLongitudeDeg(), node.getLatitudeDeg() ) );

                geods.push_back( node );                
            }

            tg_insert_polygon(cdt, poly);
        }

        if ( debug ) {
            sprintf( layer, "nodes_%03d", id );
            //tgShapefile::FromGeodList( geods, false, "./tridbg", layer, "nodes" );
        }
        
        assert(cdt.is_valid());
        tg_mark_domains( cdt );

        int count=0;
        geods.clear();
        for (CDTPlus::Finite_faces_iterator fit=cdt.finite_faces_begin(); fit!=cdt.finite_faces_end(); ++fit) {
            if ( fit->info().in_domain() ) {
                SG_LOG( SG_GENERAL, SG_DEBUG, "Tess : face   in domain");

                Triangle_2 tri = cdt.triangle(fit);

                SGGeod p0 = SGGeod::fromDeg( to_double(tri.vertex(0).x()), to_double(tri.vertex(0).y()) );
                SGGeod p1 = SGGeod::fromDeg( to_double(tri.vertex(1).x()), to_double(tri.vertex(1).y()) );
                SGGeod p2 = SGGeod::fromDeg( to_double(tri.vertex(2).x()), to_double(tri.vertex(2).y()) );

                geods.push_back( p0 );
                geods.push_back( p1 );
                geods.push_back( p2 );
                
                /* Check for Zero Area before inserting */
                //if ( !SGGeod_isEqual2D( p0, p1 ) && !SGGeod_isEqual2D( p1, p2 ) && !SGGeod_isEqual2D( p0, p2 ) ) {
                    AddTriangle( p0, p1, p2 );
                //} else {
                //    SG_LOG( SG_GENERAL, SG_BULK, "tesselation dropping ZAT" );
                //}

                ++count;
            } else {
                SG_LOG( SG_GENERAL, SG_DEBUG, "Tess : face not in domain");
            }
        }
        
        if ( debug ) {
            sprintf( layer, "tris_%03d", id );
            //tgShapefile::FromPolygon(*this, false, true, "./tridbg", layer, "tris" );
        
            sprintf( layer, "tri_nodes_%03d", id );
            //tgShapefile::FromGeodList( geods, false, "./tridbg", layer, "after_tri" );                
        }
    } else {
        SG_LOG( SG_GENERAL, SG_DEBUG, "Tess : no contours" );
    }
}

void tgPolygon::Tesselate( const std::vector<SGGeod>& extra, bool debug )
{
    CDTPlus cdt;
    std::vector<SGGeod> geods;
    // char layer[256];
    
    SG_LOG( SG_GENERAL, SG_DEBUG, "Tess " << id );
        
    // Bail right away if polygon is empty
    if ( contours.size() != 0 ) {
        // insert each polygon as a constraint into the triangulation
        for ( unsigned int c = 0; c < contours.size(); c++ ) {
            tgContour contour = contours[c];
            Polygon_2 poly;

            for (unsigned int n = 0; n < contour.GetSize(); n++ ) {
                SGGeod node = contour.GetNode(n);
                SG_LOG( SG_GENERAL, SG_DEBUG, "Tess : Adding GEOD " << node);
                poly.push_back( Point( node.getLongitudeDeg(), node.getLatitudeDeg() ) );

                geods.push_back( node );                
            }

            tg_insert_polygon(cdt, poly);
        }
        
        assert(cdt.is_valid());
        tg_mark_domains( cdt );

        int count=0;
        geods.clear();
        for (CDTPlus::Finite_faces_iterator fit=cdt.finite_faces_begin(); fit!=cdt.finite_faces_end(); ++fit) {
            if ( fit->info().in_domain() ) {
                SG_LOG( SG_GENERAL, SG_DEBUG, "Tess : face   in domain");

                Triangle_2 tri = cdt.triangle(fit);

                SGGeod p0 = SGGeod::fromDeg( to_double(tri.vertex(0).x()), to_double(tri.vertex(0).y()) );
                SGGeod p1 = SGGeod::fromDeg( to_double(tri.vertex(1).x()), to_double(tri.vertex(1).y()) );
                SGGeod p2 = SGGeod::fromDeg( to_double(tri.vertex(2).x()), to_double(tri.vertex(2).y()) );

                geods.push_back( p0 );
                geods.push_back( p1 );
                geods.push_back( p2 );
                
                /* Check for Zero Area before inserting */
                AddTriangle( p0, p1, p2 );

                ++count;
            } else {
                SG_LOG( SG_GENERAL, SG_DEBUG, "Tess : face not in domain");
            }
        }        
    } else {
        SG_LOG( SG_GENERAL, SG_DEBUG, "Tess : no contours" );
    }
}
